////
////  Image_Alignment.cpp
////  markerARLib
////
////  Created by CvidKal on 16/4/14.
////  Copyright © 2016年 CvidKal. All rights reserved.
////
//
//#include "Image_Alignment.hpp"
//
//
//using namespace cv;
//
//
//using namespace std;
//
//
//template <int SIDE,int STRIDE>
//class BlockIdx
//{
//  static_assert(SIDE>0&&SIDE%2 == 1,"Side must be odd and positive!");
//  int idx[SIDE][SIDE];
//  int * idxFromOrigin = (int *)idx;
//  int halfSide;
//  int centerIdx_;
//public:
//  BlockIdx()
//  {
//    halfSide = SIDE/2;
//    for (int i=-halfSide; i<=halfSide; ++i)
//    {
//      for (int j=-halfSide; j<=halfSide; ++j)
//      {
//        idx[i][j] = i*STRIDE+j;
//      }
//    }
//    centerIdx_ = halfSide*STRIDE+halfSide;
//  }
//  inline int getOffsetFromCenter(int x,int y)
//  {
//    assert(x>=-halfSide&&x<=halfSide&&y>=halfSide&&y<=halfSide);
//    return idx[y][x];
//  }
//  
//  inline int getOffsetFromOrigin(int offsetFromOrigin)
//  {
//    assert(offsetFromOrigin>=0&&offsetFromOrigin<SIDE*SIDE);
//    return idxFromOrigin[offsetFromOrigin];
//  }
//  inline int centerIdx()
//  {
//    return centerIdx_;
//  }
//};
//
//
//
//
//
//
//inline void computeAffine(const Vector6f &p,float x,float y,float &x1,float &y1)
//{
//  x1 =(1+p[0])*x+p[1]*y+p[2];
//  y1 =p[3]*x+(1+p[4])*y+p[5];
//}
//
//inline void applyAffine(const Vector6f &p,const Eigen::Vector2f&xy_template,Eigen::Vector2f&xy_cur)
//{
//  xy_cur.x() =(1+p[0])*xy_template.x()+p[1]*xy_template.y()+p[2];
//  xy_cur.y() =p[3]*xy_template.x()+(1+p[4])*xy_template.y()+p[5];
//}
//
//
//inline void updateAffine(Vector6f &p,Vector6f &dp)
//{
//  
//  Eigen::Matrix2f deltaA;
//  deltaA(0,0) = 1+dp[0];
//  deltaA(1,1) = 1+dp[4];
//  deltaA(0,1) = dp[1];
//  deltaA(1,0) = dp[3];
//  Eigen::Matrix2f A0;
//  A0(0,0) = 1+p[0];
//  A0(0,1) = p[1];
//  A0(1,0) = p[3];
//  A0(1,1) = 1+p[4];
//  Eigen::Vector2f t0,deltat;
//  t0(0) = p[2]; t0(1) = p[5];
//  deltat(0) = dp[2]; deltat(1) = dp[5];
//  auto A_ = A0*deltaA.inverse();
//  auto t_ = t0-A_*deltat;
//  p[0] = A_(0,0)-1; p[1] = A_(0,1); p[2] = t_(0);
//  p[3] = A_(1,0); p[4] = A_(1,1)-1; p[5] = t_(1);
//  
//};
//
//
//
//bool function_f(const Vector6f &p,const Patch &patch_without_border,const int patch_size,const Eigen::Vector2f &xy_template,const Mat &current_img,int centerIdx,
//                Eigen::Matrix<float,Eigen::Dynamic,1> &f)
//{
//  int half_patch = patch_size/2;
//  float sum=0;
//  for (int y=-half_patch; y<=half_patch; ++y)
//  {
//    for (int x=-half_patch; x<=half_patch; ++x)
//    {
//      float xx = xy_template.x() + x;
//      float yy = xy_template.y() + y;
//      float xx1,yy1;
//      int row = y * patch_size + x + centerIdx;
//      
//      computeAffine(p,xx,yy,xx1,yy1);
//      
//      //prepare bilinear interpolation coeffcient
//      int lux = std::floor(xx1);
//      int luy = std::floor(yy1);
//      float wx = xx1-(float)lux;
//      float wy = yy1 - (float)luy;
//      if(lux<0||luy<0||lux>=current_img.cols||luy>=current_img.rows)
//      {
//        return false;
//      }
//      //do bilinear interpolation
//      float I = (float)current_img.at<uchar>(luy,lux)*(1-wx)*(1-wy)+(float)current_img.at<uchar>(luy,lux+1)*(1-wy)*wx+(float)current_img.at<uchar>(1+luy,1+lux)*wx*wy+(float)current_img.at<uchar>(luy+1,lux)*wy*(1-wx);
//      sum+= I;
//      f(row) =patch_without_border.val(x, y)-patch_without_border.mean-I;
////      std::cout<<"I:"<<I<<'\t'<<"f:"<<f(row)<<std::endl;
//    }
//  }
//  
//  float mean = sum/patch_size/patch_size;
//  for(int i=0;i<f.rows();++i)
//    f(i) +=mean;
//  return true;
//  
//}
//
//inline float function_F(const Eigen::Matrix<float,Eigen::Dynamic,1> &f)
//{
//  
//  return 1./2*f.transpose()*f;
//}
//
//inline void Function_l(const Eigen::Matrix<float,Eigen::Dynamic,1> &f,const Eigen::Matrix<float,Eigen::Dynamic,6>&J,const Vector6f&dp,Eigen::Matrix<float,Eigen::Dynamic,1>&l)
//{
//  l = f+J*dp;
//}
//
//
//inline float Function_L(const Eigen::Matrix<float, Eigen::Dynamic, 1>&l)
//{
//  return 1.0/2*l.transpose()*l;
//}
//
//void align2D_Affine(
//                    const Eigen::Vector2f &xy_template,
//                    const Patch &patch_without_border,
//                    Vector6f &p,
//                    const Mat &current_img,
//                    int patch_size,
//                    Eigen::Vector2f &xy_cur,
//                    bool verbose
//                    )
//{
//  //#define debugImg
//  
//  const double epsilon = 0.1;
//  const double epsilon2 = 0.01;
//  const int maxIter=100;
//  bool convergence = false;
//  
//  
//  const int PatchSize = patch_without_border.PatchSize;
//  Eigen::Vector2f uv_last;
//  Eigen::Vector2f uv_cur;
//  Eigen::Vector2f uv_step;
//  BlockIdx<PatchSize, PatchSize> block;
//  
//  
//  //step1:compute the gradient of template patch(this is constant during the iteration)
//  CV_Assert(current_img.type() == CV_8U);
//  int half_patch = patch_size/2;
//  //  const short * porigin = patch_with_border+origin_offset_with_border;
//  
//  int pixel_num = PatchSize*PatchSize;
//  
//  Eigen::Matrix<float, Eigen::Dynamic, 6> J;
//  J.resize(pixel_num, 6);
//  Eigen::Matrix<float,6,6> A;
//  Eigen::Matrix<float,6,1> b,old_b;
//  Eigen::Matrix<float, 6, 1> dp;
//  Eigen::Matrix<float,6,1> p_old;
//  Eigen::Matrix<float,6,1> step;
//  Eigen::Matrix<float,Eigen::Dynamic,1> f;
//  f.resize(pixel_num,1);
//  A.setZero();
//  b.setZero();
//  
//#ifdef debugImg
//  Mat templateImg = Mat(patch_size,patch_size,CV_8U);
//  Mat warpImg = Mat(patch_size,patch_size,CV_8U);
//#endif
//  
//  //precompute Hessian and Jacobian
//  for (int y=-half_patch; y<=half_patch; ++y)
//  {
//    Eigen::Matrix<float, 1, 6> JJ;
//    
//    for (int x=-half_patch; x<=half_patch; ++x)
//    {
//      float dx = patch_without_border.gradient(x,y).gx;
//      float dy = patch_without_border.gradient(x,y).gy;
//      float xx = xy_template.x()+x;
//      float yy = xy_template.y()+y;
//      int row = y*patch_size+x+block.centerIdx();
//      JJ(0) = xx*dx; JJ(1) = yy*dx; JJ(2) = dx;
//      JJ(3) = xx*dy; JJ(4) = yy*dy; JJ(5) = dy;
//      A += JJ.transpose()*JJ;
//      J.row(row) = JJ;
//    }
//  }
//  uv_last = xy_template;
//  //compute initial b
////  applyAffine(p, uv_last, uv_cur);
////  patch cur(current_img.ptr<uchar>(uv_cur.x(),uv_cur.y()),current_img.cols);
////  imshow("template",patch_without_border.patchImg);
////  imshow("now",cur.patchImg);
////  cvWaitKey(0);
//  
//  function_f(p, patch_without_border, patch_size, xy_template, current_img, block.centerIdx(), f);
//  cout<<f<<endl;
//  cout<<"Initial Error:"<<function_F(f)<<endl;
//  cout<<J<<endl;
//  b = -J.transpose()*f;
//  float m =std::numeric_limits<float>::min();
//  for(int i=0;i<6;i++)
//  {
//    m = std::max(m,fabsf(b(i)));
//  }
//  
//  
//  convergence = m<epsilon;
//  //compute initial mu = tao * max_i{a_ii}
//  int v = 2;
//  double mu = 0;
//  double tau = 1e-3;
//  for (int i=0; i<6; ++i)
//  {
//    if(mu<A(i,i))
//    {
//      mu = A(i,i);
//    }
//  }
//  
//  mu *=tau;
//  
//  
//  //using the initial mu to compute damping J^t*J
//  Eigen::Matrix<float,6,6> dampingH;
//  Eigen::Matrix<float,6,6> invH,invH2;
//  invH = A.inverse();
//  invH2 = invH*invH;
//  
//  //here begin the iteration optimisation
//  uv_last = xy_template;
//  for (int it=0; !convergence&&it<maxIter; ++it)
//  {
//    cout<<it<<endl;
//    dampingH = (A+mu*decltype(A)::Identity()).inverse();
////    decltype(dampingH) dampingH2 = invH-mu*invH2;
////    cout<<dampingH - dampingH2<<endl;
//    //compute step dp
//    dp = dampingH*b;
//    cout<<dp<<endl;
//    //stop criteria 1: step is smaller than episilon2
//    
//    float divider = (1+dp(0))*(1+dp(3))-dp(1)*dp(2);
//    if(fabs(divider)<0.0001)
//    {
//      printf("[Warning]Degenerate Matrix\n");
//    }
//    
//    p_old = p;
//    updateAffine(p, dp);
//    applyAffine(p, uv_last, uv_cur);
//    cout<<p<<endl;
//    
//    uv_step = uv_cur - uv_last;
//    if(uv_step.norm()<epsilon2*(uv_last.norm()*epsilon2))
//    {
//        p = p_old;
//      convergence=true;
//      continue;
//    }
//    else
//    {
//      decltype(f) f_new,l;
//      f_new.resize(pixel_num,1);
//      l.resize(pixel_num,1);
//      double r;
//      double F0,F1,L1;
//      if(function_f(p, patch_without_border, patch_size, xy_template, current_img, block.centerIdx(), f_new))
//      {
//        Function_l(f, J, dp, l);
//
//         L1 = Function_L(l);
//         F0 = function_F(f);
//         if(L1>F0)
//           goto damping;
//         F1 = function_F(f_new);
//         r = (F0-F1)/(F0-L1);
//
//      }
//      else
//        r = -1;
//      
//      if(r>0)
//      {
//        uv_last = uv_cur;
//        b = J.transpose()*f_new;
//        float m =std::numeric_limits<float>::min();
//        for(int i=0;i<6;i++)
//        {
//          m = std::max(m,fabsf(b(i)));
//        }
//        if(verbose)
//        {
//          printf("It:%d error:%lf\n",it,F1);
//        }
//        f = f_new;
//        convergence = m<epsilon;
//        mu = mu*std::max(1./3,1-pow(2*r-1,3));
//      }
//      else
//      {
//      damping:
//        mu = mu *v;
//        v = 2*v;
//      }
//      
//    }
//    
//  }
//  xy_cur = uv_last;
//}